/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.team1160.breakaway.model.robotCommand;

import com.team1160.breakaway.api.Constants;
import com.team1160.breakaway.camera.TargetOrientation;
import com.team1160.breakaway.input.InputState;
import com.team1160.breakaway.model.RobotState;

/**
 *
 * @author CJ
 */
public class TeleopTaskManager
{
    //TODO Mango
    protected long lastKickTime;
    protected long lastHoldTime;

    //---------------------------------------------------------------------

    public TeleopTaskManager()
    {
        this.lastKickTime = System.currentTimeMillis();
        this.lastHoldTime = System.currentTimeMillis();
    }

    //---------------------------------------------------------------------

    public RobotCommand getRobotCommand(RobotState robotState,
                                              InputState inputState)
    {
        RobotCommand nextRobotCommand = new RobotCommand();

        getKicker(robotState, inputState, nextRobotCommand);
        getDrive(robotState, inputState, nextRobotCommand);
        //getDribbler(robotState, inputState, nextRobotCommand);
        getArm(robotState,inputState, nextRobotCommand);

        return nextRobotCommand;
    }

    protected void getArm(RobotState robotState,
                            InputState inputState,
                            RobotCommand nextRobotCommand){
        nextRobotCommand.activate_winch = inputState.getActivateWinch();
        nextRobotCommand.toggle_claw = inputState.getTrigglerClaw();
    }

    //---------------------------------------------------------------------

    protected void getDrive(RobotState robotState,
                            InputState inputState,
                            RobotCommand nextRobotCommand)
    {
        if(inputState.getOrientByCamera())
        {
            //TODO REad Camera and send to drivetrain
                orientByCamera(robotState, nextRobotCommand);
        }
        else
        {
            this.arcadeDrive(inputState.getYAxis(),
                             inputState.getXAxis(),
                             nextRobotCommand,
                             Constants.SQUARE_JOYSTICK_INPUTS);
        }

        nextRobotCommand.motor_stop = inputState.isStopped();
        nextRobotCommand.motors_stopped = inputState.getStoppedMotor();

        //Safety Mode
        if(inputState.getSafety()){
            nextRobotCommand.dt_left_speed = getSafetyLimits(nextRobotCommand.getDriveLeft());
            nextRobotCommand.dt_right_speed = getSafetyLimits(nextRobotCommand.getDriveRight());
        }
    }

    //---------------------------------------------------------------------

    protected double getSafetyLimits(double speed){
        if(speed < -Constants.SPEED_MAX_SAFETY){
            return -Constants.SPEED_MAX_SAFETY;
        }else if(speed > Constants.SPEED_MAX_SAFETY){
            return Constants.SPEED_MAX_SAFETY;
        }else{
            return speed;
        }
    }

    //---------------------------------------------------------------------

    protected void orientByCamera(RobotState robotState,
                                  RobotCommand nextRobotCommand)
    {
        //Right is positive
        if(robotState.getTargetOrientation() == TargetOrientation.RIGHT){
            nextRobotCommand.dt_left_speed = Constants.CAMERA_SPEED_INGREMENT * robotState.getNormalizedTarget().getX();
            nextRobotCommand.dt_right_speed = Constants.CAMERA_SPEED_INGREMENT * -robotState.getNormalizedTarget().getX();
        }else if(robotState.getTargetOrientation() == TargetOrientation.LEFT){
            nextRobotCommand.dt_left_speed = Constants.CAMERA_SPEED_INGREMENT * -robotState.getNormalizedTarget().getX();
            nextRobotCommand.dt_right_speed = Constants.CAMERA_SPEED_INGREMENT * robotState.getNormalizedTarget().getX();
        }else{
            nextRobotCommand.dt_left_speed = 0.0;
            nextRobotCommand.dt_right_speed = 0.0;
        }
    }

    //---------------------------------------------------------------------

    protected void arcadeDrive(double speed,
                               double turn,
                               RobotCommand nextRobotCommand,
                               boolean squareInputs)
    {
        double leftSide;
        double rightSide;

        if(squareInputs)
        {
            if(speed < 0)
                speed *= -speed;
            else
                speed *= speed;

            if(turn < 0)
                turn *= -turn;
            else
                turn*= turn;
        }

        if(speed > 0.0)
        {
            //We want to go backward
            if(turn > 0.0){
                //We want to go back-right(Left is faster than Right)e.g. more neg
                leftSide = speed - turn;
                rightSide = Math.max(speed, turn);
            }else{
                //We want to go back-left(Right is more neg)
                leftSide = Math.max(speed, -turn);
                rightSide = speed + turn;
            }
        }
        else
        {
            //We want to go forward
            if(turn > 0.0)
            {
                //Left Side is Faster
                //Negatives are here becuase of the neg speed
                //Also for when turn is neg
                leftSide = -Math.max(-speed, turn);
                rightSide = speed + turn;
            }
            else
            {
                //Right Side is Faster(Math.Max!)
                leftSide = speed - turn;
                rightSide = -Math.max(-speed, -turn);
            }
        }

        nextRobotCommand.dt_left_speed = leftSide;
        nextRobotCommand.dt_right_speed = rightSide;
    }

    //---------------------------------------------------------------------

    protected void getKicker(RobotState robotState,
                             InputState inputState,
                             RobotCommand nextRobotCommand)
    {
        
        if(inputState.getToggleShooter()){
            if(robotState.isKickerKicking()){
                nextRobotCommand.extend_shooter = false;
                this.lastHoldTime = System.currentTimeMillis();
            }
            else{
                nextRobotCommand.extend_shooter = true;
                this.lastKickTime = System.currentTimeMillis();
            }
        }else{
            nextRobotCommand.extend_shooter = robotState.isKickerKicking();
        }
        
        if(!robotState.isKickerKicking() && System.currentTimeMillis() - this.lastHoldTime >= Constants.DELAY_KICKER_HOLDING){
            nextRobotCommand.hold_shooter = true;
        }else if(robotState.isKickerKicking() && System.currentTimeMillis() - this.lastKickTime >= Constants.DELAY_KICKER_RELEASE){
            nextRobotCommand.hold_shooter = false;
        }else{
            nextRobotCommand.hold_shooter = robotState.latched();
        }
    }

    //---------------------------------------------------------------------

    protected void getDribbler(RobotState robotState,
                               InputState inputState,
                               RobotCommand nextRobotCommand)
    {
        
        nextRobotCommand.dribbler_on = inputState.getDribblerOn();

        if(inputState.getDribblerSpeed() == Constants.DR_SENT)
            nextRobotCommand.dribbler_speed = Constants.S_DR_MOTOR;
        else
            nextRobotCommand.dribbler_speed = inputState.getDribblerSpeed();
    }

    //---------------------------------------------------------------------
}
